#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#ifndef COMPLETEDRIVER
#include "drivers/CompleteDriver.h"
#endif

task main {
  HTSMUXinit();
  HTSMUXscanPorts (SMUX_1);
  HTSMUXscanPorts (SMUX_2);
  HTEOPDsetLongRange(EOPD_L);
  HTEOPDsetLongRange(EOPD_R);
  servo[frontarm] = 0;
  servo[backarm] = 244;
  servo[dispencer] = 5;
  wait1Msec(50);
  HTGYROreadCal(GYROPOS);
  StartTask(sensorRead);
  startkalmin();
  bDisplayDiagnostics = false;
  kalmindebugdisplay = false;
  eraseDisplay();
  while (nNxtButtonPressed != kEnterButton){ };
  /*
  alignspincompass(0):
  wait1Msec(10000);
  alignspinEOPD();
  wait1Msec(10000);
  alignDist(16);
  wait1Msec(10000);
  leftright(LEFT);
  wait1Msec(10);
  */
  allignHeight(MIN_HEIGHT_ARM + 90);
  _alignSpinCompass(0);
  _alignSpinEOPD();
  alignDist(43);
  StartEOPD();
  leftRight(RIGHTSD);
  allignHeight(RETRIEVING_MIDDLE);
  nMotorEncoder[b] = 0;
  forward (15);
  while (nMotorEncoder[b] < 10){
  }
  stopmot();
  nxtDisplayTextLine(2, "%d", PotReading());

  motor[e] = -50;
  while (true);
}
